#include <stdio.h>
#include <stdlib.h>
#include <string.h>

/* Environment header files. */
#include "drivers/pm.h"
#include "drivers/gpio.h"
#include "drivers/pwm.h"
#include "drivers/adc.h"

/* Scheduler header files. */
#include "os/FreeRTOS.h"
#include "os/task.h"

/* Demo file headers. */
#include "os/serial.h"
#include "os/PollQ.h"
#include "os/dynamic.h"
#include "os/BlockQ.h"
#include "os/death.h"
#include "os/flop.h"


#include "terminal.h"
#include "asserv.h"
#include "deplacement.h"
#include "astar.h"
#include "capteur.h"

#define mainLOOP_TASK_PRIORITY	  ( tskIDLE_PRIORITY + 2 )
#define mainLOOP_PERIODE          ( ( portTickType ) 100 / portTICK_RATE_MS  )

#define SECONDE					( ( portTickType ) 1000 / portTICK_RATE_MS  )


/*
 * The task that executes at the highest priority and calls
 * prvCheckOtherTasksAreStillRunning().  See the description at the top
 * of the file.
 */
static void mainLoop( void *pvParameters );

/* Start PLL0, enable a generic clock with PLL0 output then switch main clock to PLL0 output.
   All calculations in this function suppose that the Osc0 frequency is 12MHz. */
void startPll0(volatile avr32_pm_t* pm)
{
  /* Setup PLL0 on Osc0, mul=3 ,no divisor, lockcount=16, ie. 12Mhzx8 = 96MHz output */
  /*void pm_pll_setup(volatile avr32_pm_t* pm,
                  unsigned int pll,
                  unsigned int mul,
                  unsigned int div,
                  unsigned int osc,
                  unsigned int lockcount) {
   */
  pm_pll_setup(pm,
               0,   // use PLL0
               7,   // MUL=7 in the formula
               1,   // DIV=1 in the formula
               0,   // Sel Osc0/PLL0 or Osc1/PLL1
  //             63); // lockcount in main clock for the PLL wait lock
               50); // lockcount in main clock for the PLL wait lock

  /*
   This function will set a PLL option.
   *pm Base address of the Power Manager (i.e. &AVR32_PM)
   pll PLL number 0
   pll_freq Set to 1 for VCO frequency range 80-180MHz, set to 0 for VCO frequency range 160-240Mhz.
   pll_div2 Divide the PLL output frequency by 2 (this settings does not change the FVCO value)
   pll_wbwdisable 1 Disable the Wide-Bandith Mode (Wide-Bandwith mode allow a faster startup time and out-of-lock time). 0 to enable the Wide-Bandith Mode.
  */
  /* PLL output VCO frequency is 96MHz. We divide it by 2 with the pll_div2=1. This enable to get later main clock to 48MHz */
  pm_pll_set_option(pm, 0, 1, 1, 0);

  /* Enable PLL0 */
  /*
    void pm_pll_enable(volatile avr32_pm_t* pm,
                  unsigned int pll) {
  */
  pm_pll_enable(pm,0);


  /* Wait for PLL0 locked */
  pm_wait_for_pll0_locked(pm) ;

  /* Setup generic clock on PLL0, with Osc0/PLL0, no divisor */
  /*
  void pm_gc_setup(volatile avr32_pm_t* pm,
                  unsigned int gc,
                  unsigned int osc_or_pll, // Use Osc (=0) or PLL (=1)
                  unsigned int pll_osc, // Sel Osc0/PLL0 or Osc1/PLL1
                  unsigned int diven,
                  unsigned int div) {
  */
  pm_gc_setup(pm,
              0,
              1,  // Use Osc (=0) or PLL (=1), here PLL
              0,  // Sel Osc0/PLL0 or Osc1/PLL1
              0,  // disable divisor
              0); // no divisor

  /* Enable Generic clock */
  pm_gc_enable(pm, 0);

  /* Divide PBA clock by 2 from main clock (PBA clock = 48MHz/2 = 24MHz).
     Pheripheral Bus A clock divisor enable = 1
     Pheripheral Bus A select = 0
     Pheripheral Bus B clock divisor enable = 0
     Pheripheral Bus B select = 0
     High Speed Bus clock divisor enable = 0
     High Speed Bus select = 0
  */
  pm_cksel(pm, 1, 0, 0, 0, 0, 0);

  pm_switch_to_clock(pm, AVR32_PM_MCSEL_PLL0); /* Switch main clock to 48MHz */
}


int main( void )
{
	volatile avr32_pm_t* pm = &AVR32_PM;
	/* Start the crystal oscillator 0 and switch the main clock to it. */
	pm_switch_to_osc0(pm, FOSC0, OSC0_STARTUP);

	startPll0(pm);

	/* Start the standard demo tasks.  See the WEB documentation for more
	information. */
	terminalInit();

	/* Start the demo tasks defined within this file, specifically the check
	task as described at the top of this file. */
	xTaskCreate( mainLoop, (const signed portCHAR *)"Main", configMINIMAL_STACK_SIZE, NULL, mainLOOP_TASK_PRIORITY, NULL );

	asservInit();
	astarInit();
	capteursInit();
	evitInit();

	/* Start the scheduler. */
	vTaskStartScheduler();

	/* Will only get here if there was insufficient memory to create the idle
	task. */



	return 0;


}
/*-----------------------------------------------------------*/

/*!
 * \brief The task function for the "Check" task.
 */
static void mainLoop( void *pvParameters )
{
	/* The parameters are not used.  Prevent compiler warnings. */
	( void ) pvParameters;


	while(1)
	{
		gpio_tgl_gpio_pin(LED0);
	    vTaskDelay(mainLOOP_PERIODE);
	}
}
